﻿using System;
using System.Collections.Generic;

namespace g4
{
    public static class MatrixUtil
    {

        public static double[] MakeIdentity3x3(double a, double b, double c)
        {
            return new double[9] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
        }
        public static void SetIdentity3x3(double[] M, double a, double b, double c)
        {
            Array.Clear(M, 0, 9);
            M[0] = M[4] = M[8] = 1;
        }


        public static double[] MakeDiagonal3x3(double a, double b, double c)
        {
            return new double[9] { a, 0, 0, 0, b, 0, 0, 0, c };
        }
        public static void SetDiagonal3x3(double[] M, double a, double b, double c)
        {
            Array.Clear(M, 0, 9);
            M[0] = a; M[4] = b; M[8] = c;
        }

        // assumption is matrix is row-major
        public static double Determinant3x3(double[] M)
        {
            double co00 = M[4] * M[8] - M[5] * M[7];
            double co10 = M[5] * M[6] - M[3] * M[8];
            double co20 = M[3] * M[7] - M[4] * M[6];
            double det = M[0] * co00 + M[1] * co10 + M[2] * co20;
            return det;
        }


        public static void Transpose3x3(double[] M)
        {
            double tmp = M[1]; M[1] = M[3]; M[3] = tmp;
            tmp = M[2]; M[2] = M[6]; M[6] = tmp;
            tmp = M[5]; M[5] = M[7]; M[7] = tmp;
        }
        public static void Transpose3x3(double[] M, double[] MTranspose)
        {
            MTranspose[0] = M[0];
            MTranspose[1] = M[3];
            MTranspose[2] = M[6];
            MTranspose[3] = M[1];
            MTranspose[4] = M[4];
            MTranspose[5] = M[7];
            MTranspose[6] = M[2];
            MTranspose[7] = M[5];
        }

        // C = A * B
        public static void Multiply3x3(double[] A, double[] B, double[] C)
        {
            C[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
            C[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
            C[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
            C[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
            C[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
            C[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
            C[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
            C[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
            C[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
        }


        public static Vector3d Multiply3x3(double[] M, Vector3d vec)
        {
            return new Vector3d(
                M[0] * vec.x + M[1] * vec.y + M[2] * vec.z,
                M[3] * vec.x + M[4] * vec.y + M[5] * vec.z,
                M[6] * vec.x + M[7] * vec.y + M[8] * vec.z
                );
        }

    }
}
